#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <fstream>
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>

int i = 0;
void ImageCallback(const sensor_msgs::CompressedImage::ConstPtr &img)
{
    printf("img get ...\n");
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(boost::make_shared<sensor_msgs::CompressedImage>(*img), "bgr8");
    }
    catch (cv_bridge::Exception &e)
    {
        return;
    }
    cv::Mat raw_image = cv_ptr->image;
    cv::resize(raw_image, raw_image, cv::Size{1920, 1080});
    cv::imwrite("/home/zwh/work_space/18xx/calibration-tool/TEST_DATA/nanchuan/hw3_2_c75/" + std::to_string(i) + ".jpg", raw_image);
    i++;
}

void ImageCallback(const sensor_msgs::Image::ConstPtr &img)
{
    printf("img get ...\n");
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(boost::make_shared<sensor_msgs::Image>(*img), "bgr8");
    }
    catch (cv_bridge::Exception &e)
    {
        return;
    }
    cv::Mat raw_image = cv_ptr->image;
    cv::resize(raw_image, raw_image, cv::Size{1920, 1080});
    cv::imwrite("/media/zwh/T7/ZWH/bags/2023/zrh/3lidae3camera/" + std::to_string(i) + ".jpg", raw_image);
    i++;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "cameraCalib");
    ros::NodeHandle nh("~");
    ros::Subscriber sub_img = nh.subscribe<sensor_msgs::Image>("/camera75", 1, &ImageCallback);
//    ros::Subscriber sub_cimg = nh.subscribe<sensor_msgs::CompressedImage>("/camera31_bgr/compressed", 1, &ImageCallback);

    ros::spin();
    return 0;
}